﻿/*                       NOTICE
 * 
 * This software was produced for the U. S. Government
 * under Contract No. FA8721-10-C-0001, and is
 * subject to the Rights in Noncommercial Computer Software
 * and Noncommercial Computer Software Documentation Clause
 * (DFARS) 252.227-7014 (JUN 1995)
 * 
 * Approved for Public Release: 11-2435
 * © 2011 The MITRE Corporation. All Rights Reserved. 
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 * http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;
using GRIP;
using TUIO;
using System.Windows.Forms;

namespace VisualTester
{
    public partial class MainWindow : Window
    {
        Timer timer;
        KinectAdapter kinect;
        private TuioContact[][] kinect_points;
        byte[] depthFrame32 = new byte[640 * 480 * 4];
        private Brush blueBrush = new SolidColorBrush(Color.FromRgb(0, 0, 255));
        private Brush greenBrush = new SolidColorBrush(Color.FromRgb(0, 255, 0));
        private Brush redBrush = new SolidColorBrush(Color.FromRgb(255, 0, 0));
        private Brush yellowBrush = new SolidColorBrush(Color.FromRgb(255, 255, 0));
        private List<TuioContact> fingersLeft;
        private List<TuioContact> fingersRight;
        private Brush[] fingerBrushes = { new SolidColorBrush(Color.FromRgb(0, 255, 0)),
                                          new SolidColorBrush(Color.FromRgb(155, 255, 0)),
                                          new SolidColorBrush(Color.FromRgb(255, 255, 0)),
                                          new SolidColorBrush(Color.FromRgb(255, 255, 155)),
                                          new SolidColorBrush(Color.FromRgb(255, 255, 255))
                                        };

        private List<TextBlock> labelsLeft = new List<TextBlock>();
        private List<TextBlock> labelsRight = new List<TextBlock>();

        public MainWindow()
        {
            InitializeComponent();
        }

        private void Window_Loaded(object sender, EventArgs e)
        {
            ConfigParser parser = new ConfigParser("config.xml");
            KinectSettings kinectSettings = new KinectSettings();
            FingerSettings fingerSettings = new FingerSettings();
            try
            {
                parser.LoadConfiguration(kinectSettings);
                parser.LoadConfiguration(fingerSettings);
            }
            catch (Exception ex)
            {
                Console.WriteLine(ex);
            }
            kinect = new KinectAdapter(kinectSettings, fingerSettings);

            kinect_points = new TuioContact[][]
            {
                 new TuioContact[] { kinect.Head, kinect.ShoulderCenter, kinect.Spine, kinect.HipCenter },
                 new TuioContact[] { kinect.ShoulderCenter, kinect.ShoulderLeft, kinect.ElbowLeft, kinect.WristLeft, kinect.HandLeft },
                 new TuioContact[] { kinect.ShoulderCenter, kinect.ShoulderRight, kinect.ElbowRight, kinect.WristRight, kinect.HandRight },
                 new TuioContact[] { kinect.HipCenter, kinect.HipLeft, kinect.KneeLeft, kinect.AnkleLeft, kinect.FootLeft },
                 new TuioContact[] { kinect.HipCenter, kinect.HipRight, kinect.KneeRight, kinect.AnkleRight, kinect.FootRight }
            };

            fingersLeft = new List<TuioContact>(new TuioContact[] { kinect.ThumbLeft, kinect.IndexFingerLeft, kinect.MiddleFingerLeft, kinect.RingFingerLeft, kinect.PinkyLeft });
            fingersRight = new List<TuioContact>(new TuioContact[] { kinect.ThumbRight, kinect.IndexFingerRight, kinect.MiddleFingerRight, kinect.RingFingerRight, kinect.PinkyRight });
            {
                TextBlock thumbLabel = new TextBlock();
                thumbLabel.Foreground = yellowBrush;
                thumbLabel.Text = "Thumb";
                TextBlock indexLabel = new TextBlock();
                indexLabel.Foreground = yellowBrush;
                indexLabel.Text = "Index";
                TextBlock middleLabel = new TextBlock();
                middleLabel.Foreground = yellowBrush;
                middleLabel.Text = "Middle";
                TextBlock ringLabel = new TextBlock();
                ringLabel.Foreground = yellowBrush;
                ringLabel.Text = "Ring";
                TextBlock pinkyLabel = new TextBlock();
                pinkyLabel.Foreground = yellowBrush;
                pinkyLabel.Text = "Pinky";
                labelsLeft.Add(thumbLabel);
                labelsLeft.Add(indexLabel);
                labelsLeft.Add(middleLabel);
                labelsLeft.Add(ringLabel);
                labelsLeft.Add(pinkyLabel);
            }
            {
                TextBlock thumbLabel = new TextBlock();
                thumbLabel.Foreground = yellowBrush;
                thumbLabel.Text = "Thumb";
                TextBlock indexLabel = new TextBlock();
                indexLabel.Foreground = yellowBrush;
                indexLabel.Text = "Index";
                TextBlock middleLabel = new TextBlock();
                middleLabel.Foreground = yellowBrush;
                middleLabel.Text = "Middle";
                TextBlock ringLabel = new TextBlock();
                ringLabel.Foreground = yellowBrush;
                ringLabel.Text = "Ring";
                TextBlock pinkyLabel = new TextBlock();
                pinkyLabel.Foreground = yellowBrush;
                pinkyLabel.Text = "Pinky";
                labelsRight.Add(thumbLabel);
                labelsRight.Add(indexLabel);
                labelsRight.Add(middleLabel);
                labelsRight.Add(ringLabel);
                labelsRight.Add(pinkyLabel);
            }

            timer = new Timer();
            timer.Interval = 40;
            timer.Tick += new EventHandler(timer_Elapsed);
            timer.Start();
        }

        void timer_Elapsed(object sender, EventArgs e)
        {
            convertDepthFrame(kinect.getDepthFrame());
            skeleton.Background = new ImageBrush(BitmapSource.Create(
                640, 480, 96, 96, PixelFormats.Bgr32, null, depthFrame32, 2560));
            drawSkeleton();
        }

        public void drawSkeleton()
        {
            skeleton.Children.Clear();
            TuioContact[] contacts;
            TuioContact contact;
            PointCollection points;
            List<TuioContact> activeContacts = kinect.getContacts();

            for (int i = 0; i < kinect_points.Length; i++)
            {
                points = new PointCollection();
                contacts = kinect_points[i];
                for (int j = 0; j < contacts.Length; j++)
                {
                    contact = contacts[j];
                    if(contact.active)
                        points.Add(new Point((1 - contact.xpos) * 640, contact.ypos * 480));
                }
                drawLine(points);
            }

            for (int i = 0; i < activeContacts.Count; i++)
            {
                contact = activeContacts[i];
                if (contact.active)
                    drawPoint(contact, blueBrush);
            }

            //Used for debugging hand tracking
            /*
            points = new PointCollection();
            float handX = (1 - kinect.HandLeft.xpos) * 640;
            float handY = kinect.HandLeft.ypos * 480;
            points.Add(new Point(handX - 70, handY - 70));
            points.Add(new Point(handX + 70, handY - 70));
            points.Add(new Point(handX + 70, handY + 70));
            points.Add(new Point(handX - 70, handY + 70));
            points.Add(points[0]);
            drawLine(points);
            */

            drawHand(fingersLeft, kinect.HandLeft, labelsLeft);
            drawHand(fingersRight, kinect.HandRight, labelsRight);
        }

        private void drawPoint(TuioContact contact, Brush brush)
        {
            Ellipse ellipse = new Ellipse();
            ellipse.Width = 12;
            ellipse.Height = 12;
            Canvas.SetLeft(ellipse, (1 - contact.xpos) * 640 - 6);
            Canvas.SetTop(ellipse, contact.ypos * 480 - 6);
            ellipse.Stroke = brush;
            ellipse.StrokeThickness = 12;
            skeleton.Children.Add(ellipse);
        }

        private void drawLine(PointCollection points)
        {
            Polyline polyline = new Polyline();
            polyline.Points = points;
            polyline.Stroke = redBrush;
            polyline.StrokeThickness = 5;
            skeleton.Children.Add(polyline);
        }

        private void drawHand(List<TuioContact> fingerPoints, TuioContact hand, List<TextBlock> labels)
        {
            for (int i = 0; i < fingerPoints.Count; i++)
            {
                TuioContact finger = fingerPoints[i];
                if (finger.active)
                {
                    PointCollection points = new PointCollection();
                    points.Add(new Point((1 - hand.xpos) * 640, hand.ypos * 480));
                    points.Add(new Point((1 - finger.xpos) * 640, finger.ypos * 480));

                    Canvas.SetLeft(labels[i], (1 - finger.xpos) * 640);
                    Canvas.SetTop(labels[i], finger.ypos * 480 - 15);
                    skeleton.Children.Add(labels[i]);

                    drawLine(points);
                    drawPoint(finger, greenBrush);
                }
            }
        }

        // Converts a 16-bit grayscale depth frame which includes player indexes into a 32-bit frame
        // that displays different players in different colors
        byte[] convertDepthFrame(byte[] depthFrame16)
        {
            for (int i16 = 0, i32 = 0; i16 < depthFrame16.Length && i32 < depthFrame32.Length; i16 += 2, i32 += 4)
            {
                int realDepth = (depthFrame16[i16 + 1] << 8) | depthFrame16[i16];
                // transform 13-bit depth information into an 8-bit intensity appropriate
                // for display (we disregard information in most significant bit)
                byte intensity = (byte)((255 - (255 * realDepth / 0x0fff)) / 2);

                depthFrame32[i32 + 2] = intensity;
                depthFrame32[i32 + 1] = intensity;
                depthFrame32[i32 + 0] = intensity;
            }
            return depthFrame32;
        }
        
        private void Window_Closed(object sender, EventArgs e)
        {
            kinect.shutdown();
            Environment.Exit(0);
        }
    }
}
